/*
 www.pipoisu.com
 Copyright (c) 2011.  All rights reserved.

 File     : sensors.cpp
 Author(s): Erez Raviv

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
 

#include "sensors.h"
#include "i2cmaster.h"
#include "pipo.h"

float   sensor_data[9]        = {0};      // data from sensors.  3 int from accel(0..2) + 3 int from gyro(3..5)
int16_t sensor_data_int[9]    = {0};      // data from sensors.  3 int from accel(0..2) + 3 int from gyro(3..5)
int16_t gyro_bias[3]          = {0};      // gyro biases

uint16_t lastMagnetometerRead;

/*************************************************************************
 LP filter
*************************************************************************/
#define LP_WEIGHT 0.3
inline void lpFilter(uint8_t s, float nv)
{
    static float t; // prevent extra push/pop
//    t = lp_alpha * (nv - sensor_data[s]);
    t = nv*(LP_WEIGHT) + sensor_data[s]*(1-LP_WEIGHT);
    sensor_data[s] = t;
}

/*************************************************************************
 normalize and LP filter a sensor
*************************************************************************/
void nc_sensor(uint8_t s, uint8_t nolp=0)
{
    static float x,y,z,norm;
    x = sensor_data_int[s];
    y = sensor_data_int[s+1];
    z = sensor_data_int[s+2];

    //normalize acceleration, LP filter and copy to int array
    norm = sqrt(x*x + y*y + z*z);
    if(nolp)
    {
        sensor_data[s]   = x/norm;
        sensor_data[s+1] = y/norm;
        sensor_data[s+2] = z/norm;
    }
    else
    {
        lpFilter(s,   (x/norm));   //sensor_data[s]/2   + (x / norm)/2;
        lpFilter(s+1, (y/norm));   //sensor_data[s+1]/2 + (y / norm)/2;
        lpFilter(s+2, (z/norm));   //sensor_data[s+2]/2 + (z / norm)/2;
    }
}

/*************************************************************************
 read, calibrate, normalize, LP filter  data from sensors
*************************************************************************/
void populateSensorData()
{ 
	int16_t tmp;
	/* Axis convention/orientation
	 * http://en.wikipedia.org/wiki/Axes_conventions for info
	 * Sensors are all ENU based, oriented as below
	 * NED is the common axis convention for aircraft
	 * NED Earth frame x = North, y = East, z = towards earth centre
	 * NED aircraft frame +x in direction of travel, +y = starboard wing, +z = down
	 *
	 * Rotations in NED
	 * if you point your right hand thumb in the direction of the axis with fingers out straight
	 * then as you close your fist your fingers move with a positive rotation about that axis
	 *
	 * !!! Silkscreen is marked with ENU !!!
	 */
	
    //Gyro L3G4200D is oriented ENU
    //Accelerometer ADXL is turned 90 degrees clockwise from ENU: 
    //Magnetometer HMC5883L is rotated 180 degrees

    //read XYZ data from L3G4200D
	// read from register 0x28 + MSb set to auto increment
    i2cRead6(L3G4200D_ADDR,((0x28) | (1<<7)),(uint8_t *)&sensor_data_int[SEN_GX]); 
	
    // Convert to NED coordinates
	//sensor_data_int[SEN_GX]= (sensor_data_int[SEN_GX]);
	sensor_data_int[SEN_GY]= -(sensor_data_int[SEN_GY]);
	sensor_data_int[SEN_GZ]= -(sensor_data_int[SEN_GZ]);

    for(uint8_t i=SEN_GX; i<=SEN_GZ; i++) {
		sensor_data_int[i] -= gyro_bias[i-SEN_GX];
        sensor_data[i]	    = ((float)(sensor_data_int[i]))*L3G4200D_DPS_TO_RPS; // convert to mRAD/sec
//        lpFilter(i, (float(sensor_data_int[i]))*L3G4200D_DPS_TO_RPS);
    }
	
	
	//read XYZ data from ADXL345
    i2cRead6(ADXL_ADDR, 0x32, (uint8_t *)&sensor_data_int[SEN_AX]);
    tmp = sensor_data_int[SEN_AX];
    sensor_data_int[SEN_AX] =  -sensor_data_int[SEN_AY];
    sensor_data_int[SEN_AY] = -tmp;
	//sensor_data_int[SEN_AZ] = (sensor_data_int[SEN_AZ]);

    //nc_sensor(SEN_AX); //normalize accelerometer
	

    //read XYZ data from HMC5883L
    if(magnetometer) {
        if (i2cRead(HMC5883L_ADDR,0x09)& 1 ) { // only read when ready
            uint8_t * t = (uint8_t *)&sensor_data_int[SEN_MX];
            i2cRead6(HMC5883L_ADDR, 0x03, t);
            //HMC5883L is weird!  Y and Z are swapped as well!
            //we need to swap bytes, and do the following swaps:
            //y <= -z
            //z <= y
            //x <= -x
            //HMC5883L is big endian -> swap bytes
            
			//TODO - tidy this up, do teh byte swap and int swap in one operation i.e. sensor_data_int(xx) = +/-((t[y]<<8) | t[y+1])
			SWAP(t[0], t[1]);
            SWAP(t[2], t[3]);
            SWAP(t[4], t[5]);

            tmp = sensor_data_int[SEN_MY];
            sensor_data_int[SEN_MX] = -sensor_data_int[SEN_MX]; //rotate 180 degrees - HMC5883L
			sensor_data_int[SEN_MY] = sensor_data_int[SEN_MZ];
            sensor_data_int[SEN_MZ] = -tmp;
           
            //nc_sensor(SEN_MX, 1); //normailze magnetometer - no lp filter
        }
    }
}


/*************************************************************************
 read sensors
 populate scale and bias
*************************************************************************/
void calibrateGyro()
{
    int32_t accumulator[3] = {0};
	uint16_t counter;
    
    for (counter=0; counter<CALIB_SAMPLES; counter++)    {
        LED_RED_FLIP;  // flash red led to signify calibration
        uint16_t cg25ms = g25msec + 4; // current timer - make sure we don't read more than once every 100ms
        populateSensorData();
    
        for(uint8_t i=0; i<3; i++)
            accumulator[i] += sensor_data_int[i+SEN_GX];

        while (cg25ms != g25msec);  // wait for next 100ms period - one sample every 100ms
    }

    if(counter>0)    {
        for(uint8_t i=0; i<3; i++)
            gyro_bias[i] = accumulator[i]/counter;
    }
}


/*************************************************************************
 set up ADXL345
*************************************************************************/
uint8_t adxl_init()
{
    if(!i2cDeviceReady(ADXL_ADDR))
        return 0;

    if(i2cRead(ADXL_ADDR,0x00)!=ADXL_ID)
        return 0;

    i2cWrite(ADXL_ADDR, 0x2C, 0x0C);   //BW_Rate register  //ODR = 400Hz, BW = 200Hz
    i2cWrite(ADXL_ADDR, 0x31, 0x10);   // DATA_FORMAT register // set the "full rez" bit +-16g
    i2cWrite(ADXL_ADDR, 0x2D, (1<<3)); //PWER_CTL register //Start measure -> bit 3 on.

    return 1;
}

/*************************************************************************
 set up L3G4200D
*************************************************************************/
uint8_t l3g4200d_init()
{
    if(!i2cDeviceReady(L3G4200D_ADDR))
        return 0;

    if(i2cRead(L3G4200D_ADDR,0x0F)!=L3G4200D_ID)
        return 0;

    //i2cWrite(L3G4200D_ADDR, 0x20, 0xAF);  //CTRL_REG1  //power up device  set up data rate (400Hz ODR / BW cutoff @ 50Hz)
	//i2cWrite(L3G4200D_ADDR, 0x20, 0x4F);  //CTRL_REG1  //power up device  set up data rate (200Hz ODR / BW cutoff @ 12.5Hz)
	i2cWrite(L3G4200D_ADDR, 0x20, 0xBF);  // CTRL_REG1 - 400hz ODR, 70hz cut-off powered up x,y,z enabled
	
    //i2cWrite(L3G4200D_ADDR, 0x24, 0x13);  //CTRL_REG5  //High and low pass filters are on
	i2cWrite(L3G4200D_ADDR, 0x24, 0x00);  //CTRL_REG5  //High and low pass filters are off interrupts are off
    
	i2cWrite(L3G4200D_ADDR, 0x23, 0x90);  //CTRL_REG4  //500 dps / BDU enabled 

    //uint8_t ret = i2cRead(L3G4200D_ADDR,0x21); //CTRL_REG2 - must read this one so we don't change top 2 bits
    //i2cWrite(L3G4200D_ADDR, 0x21, (ret | 0x27)); //CTRL_REG2 // HPM1 - normal mode, cutoff - 0.2Hz in 400Hz rate
	
	//i2cWrite(L3G4200D_ADDR, 0x21, ((ret & 0XC0) | 0x06)); //CTRL_REG2 // HPM1 - normal mode, cutoff - 0.2Hz in 200Hz rate

    return 1;
}


/*************************************************************************
 set up HMC5883L or HMC5883L
*************************************************************************/
uint8_t HMC5883L_init()
{
    if(!i2cDeviceReady(HMC5883L_ADDR))
        return 0;

    if(i2cRead(HMC5883L_ADDR,0x0A)!=HMC5883L_ID1)  // check id
        return 0;

    i2cWrite(HMC5883L_ADDR,0x00,0x70); // 8-average, 15 Hz default, normal measurement
    i2cWrite(HMC5883L_ADDR,0x01,0xA0); // 4.7Gauss
    i2cWrite(HMC5883L_ADDR,0x02,0x00); // mode register - continuous measurement - clear md1 bit

    return 1;
}
